#include <ros/ros.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <Eigen/Geometry>

// 声明逆运动学函数
std::vector<double> computeIK(robot_state::RobotStatePtr kinematic_state, const robot_state::JointModelGroup* joint_model_group, const std::vector<double>& pose) {
    // 更新末端执行器的位置和方向
    Eigen::Vector3d position(pose[0], pose[1], pose[2]);
    Eigen::Quaterniond orientation(pose[3], pose[4], pose[5], pose[6]);
    Eigen::Isometry3d end_effector_state;
    end_effector_state.translation() = position;
    end_effector_state.linear() = orientation.toRotationMatrix();

    std::vector<double> joint_values;
    if (kinematic_state->setFromIK(joint_model_group, end_effector_state, 0.1)) {
        kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
    } else {
        ROS_INFO("Did not find IK solution");
    }
    return joint_values;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "kdl_kinematics_example");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    robot_model_loader::RobotModelLoader model_loader("robot_description");
    robot_model::RobotModelPtr kinematic_model = model_loader.getModel();

    robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
    kinematic_state->setToDefaultValues();
    const robot_state::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup("manipulator");

    std::vector<double> input_pose_start = {0.00627459278920944, 0.011697048892667358, 0.1426654620685569, 
    0.9063306950280158, -0.42256913167865406, -1.1063811351183932e-05, 8.942841225197472e-06};//起始点
    std::vector<double> input_pose_end = {0.006294315226467101, 0.28918579303355113, 0.2502609827289403,    
    0.9999999984636202, 4.917028402675271e-05, 3.6721275695023544e-06, -2.532900424562415e-05  };//目标点
    std::vector<double> joint_values_start = computeIK(kinematic_state, joint_model_group, input_pose_start);
    std::vector<double> joint_values_end = computeIK(kinematic_state, joint_model_group, input_pose_end);

    for (size_t i = 0; i < joint_values_start.size(); ++i) {
        ROS_INFO("Joint_start %zu: %f", i, joint_values_start[i]);
    }

    for (size_t i = 0; i < joint_values_end.size(); ++i) {
        ROS_INFO("Joint_end %zu: %f", i, joint_values_end[i]);
    }

    ros::shutdown();
    return 0;
}
